\documentclass{aiaa}
\usepackage{color, soul}
\providecommand{\e}[1]{\ensuremath{\times 10^{#1}}}
%Format Python code pretty-like
%From https://gist.github.com/kroger/6140188
\usepackage{listings}
 
\lstset{
  language=Python,
  showstringspaces=false,
  formfeed=\newpage,
  tabsize=4,
  commentstyle=\itshape,
  basicstyle=\ttfamily,
  morekeywords={models, lambda, forms}
}
 
\newcommand{\python}[2]{
  \hrulefill
  \subsection*{#1}
  \lstinputlisting{#2}
  \vspace{2em}
}


\title{ASEM 5519: Aerorobotics \\ Project 1, Phase 1}
\author{Matthew Aitken \and Aaron Buysse \and Paul Guerrie \and Steve McGuire
\and Tevis Nichols \and Will Silva}
\begin{document}
\maketitle
\begin{abstract}
This project presents an implementation of Park, Dest, and How's nonlinear guidance algorithm in an ArduPlane environment. The algorithm is implemented in Python, while the ArduPlane stock firmware code is utilized. Two levels of software simulation results are presented, a basic simulation using a Matlab environment and a software-in-the-loop (SITL) simulation using the actual autopilot code with a fixed-wing simulation engine backend.
\end{abstract}

\section{Control Algorithm Design}
\subsection{As-Published Algorithm}
The path following algorithm is based off of the algorithm developed by Park et. al, as described in their paper, \textit{A New Nonlinear Guidance Logic for Trajectory Tracking}. This algorithm creates a turn rate proportional to the angle between the velocity vector of the aircraft and the position vector between the aircraft and the reference point, as shown in \Figure{park_overview}.
\leftfig{figures/Park.pdf}{park_overview}{Illustration of the Park Control Law}

This turn rate makes the aircraft follow an arced path towards the reference point. When the angle $\eta$ is large, the arc is small, so that the aircraft turns quickly towards the path. As the aircraft gets closer to the path, the value of $\eta$ decrease, decreasing the turn rate. When the aircraft's velocity lines up with the $L_1$ vector, the turn rate is zero.

$L_1$ is a fixed distance parameter that can be tuned to meet the requirements of a specific path or aircraft. Smaller $L_1$ values track the path more closely, but aircraft dynamics will limit the algorithm's effectiveness. If $L_1$ is too small, the turn rates may be unrealizable by the system. $L_1$ is also the maximum distance from the path that this control law applies, as it is not possible to choose a reference point on the path at a distance $L_1$ from the aircraft if the minimum distance from the aircraft to the path is greater than $L_1$.

\subsection{Custom Adjustments}
The Park algorithm is implemented with several modifications needed for a practical realization.

\subsubsection{Horizontal Vs. Vertical Tracking}
 Our implementation uses the Park algorithm to control the horizontal tracking of the path, and a proportional-integral-derivative (PID) controller is used to control the altitude of the aircraft. Because of this decoupling between climb performance and turn performance, the vertical speed is not accounted for in the calculation of the tracking speed, and the horizontal tracking speed (and thus the commanded airspeed) is just the tracking speed input parameter.

\subsubsection{Path Discretization}
Another modification is the usage of a discrete set of points instead of an analytical path. The reference point is chosen as the first point on the path at or forward of the previous reference point that is at least $L_1$ meters away from the aircraft. If the interval between the points is small, this method works quite well and avoids the necessity of creating an analytic model of the path, which could be very difficult for more complex shapes. 

Another advantage of using discrete points is that it allows the aircraft to head towards the path from any distance away. When the algorithm is first initiated, it finds the closest point on the path to the aircraft, and uses that as its first reference point. While the distance between the aircraft and the closest point is greater than $L_1$, the algorithm will continue to choose that first point as its reference point, as it is the first point that is at or forward of the previous reference point that is farther than $L_1$ meters away from the aircraft. 
This means that the aircraft will turn towards the closest point and approach it at a constant speed, and once it is within $L_1$ meters of the path it will start turning to follow the path. 

\subsubsection{Command Translation}
Beneath the guidance control layer another set of controls translates the guidance commands of altitude, turn rate and airspeed into roll, pitch and throttle commands which can be entered into fly-by-wire-A (FBWA) mode as RC inputs. There are 3 separate control loops to accomplish this task. 

\subsubsection{Altitude Controller}
The first is an altitude to pitch PID controller. The proportional command is implemented simply by subtracting the aircraft's current altitude from the current altitude target and then multiplying by a gain to generate a pitch command. 
The integral control implementation is more complex. Basic forward Euler integration is performed on the altitude error to add up the integrated error and a gain is applied. One significant drawback of integral control is integrator wind up which can result in significant overshoot behavior in the controlled system. Two measures are taken in the controller to avoid this. First, integral control is only used when the aircraft is within 30m of the desired target altitude. This is achieved by manually setting the integrated error to zero when the aircraft is outside this range. Thus, the integrator is reset any time the aircraft leaves this envelope. Resetting the integrator has no practical effect on performance in these conditions because the proportional controller will saturate (command the maximum possible pitch) in these conditions; additional pitch commanded by the integrator would have no effect. However, resetting the error prevents a large integrated error from building up in the initial stages of a mission while the aircraft is traveling to the desired path. The second step taken to prevent significant overshoot related to wind-up is to clear the integrated error (by setting it to zero) when the desired path is nearly flat. In these situations, the PD elements of the controller provide good performance without the integral element; this prevents the integral control from causing an overshoot at the top of a climbing section of a path.

The derivative control term applies a gain to the difference between the current and desired climb rates.  This gain helps to damp the behavior commanded from the PI terms. The current climb rate is directly available from the aircraft state. The desired climb rate is computed by predicting the future state of the aircraft using a simple forward Euler approximation and then calling the guidance control function on that state to obtain the desired altitude at that future time. The difference between this feed-forward desired altitude and the current desired altitude is then divided by the small time delta over which the state was integrated.

A further change made in the altitude control strategy was to increase the saturation pitch angle values to $\pm$ 30 degrees in order to achieve sufficient climb and descent rates on complex paths.

\subsubsection{Throttle Controller}
To command throttle, a simple proportional control on the airspeed is augmented with a feed forward from the sine of the pitch angle. As the aircraft pitches up, gravity begins to oppose the thrust force of the aircraft. The gravity force along the body $x$ axis is proportional to the sine of the pitch angle. By commanding extra throttle with a gain applied to the sine of the pitch angle, the aircraft applies additional throttle before the airspeed decays due to climb initiation.

\subsubsection{Roll Controller}
The roll angle is commanded in a similar manner to the throttle. A feed forward roll is calculated from the requirement that the aircraft be rolled to direct a component of the lift force toward the center of the desired turn sufficient to achieve the correct turn rate. This feed-forward term is supplemented by a proportional gain applied to the difference between the current and desired turn rates. The turn rate cannot be easily measured so the yaw rate is used instead. In slowly varying wind conditions, these rates will be similar, so the yaw rate is a reasonable substitute for the true turn rate. In more complex wind environments, the turn rate could potentially be determined by filtering the turn angle data but in most cases the yaw rate is likely superior to filtering the derivative of noisy turn angle data. The yaw rate is determined by multiplying the aircraft body frame to global inertial frame rotation matrix by the angular velocity of the aircraft. Since the yaw rate is the rotation rate about the global frame $z$ axis, simply taking the third element of the resulting vector gives the yaw rate. The yaw rate is also used in the feed-forward state calculation used by the pitch controller.

\subsubsection{Conversion to RC Channel Commands}
Once the desired angles are determined, each angle is converted to an RC channel command. These commands are determined based on the channel limits in the aircraft parameter files, then translating these limits to the saturation limits of the aircraft such that a maximum RC command yields the saturation limit of the input. For example, applying a command value of 2000 to RC channel 1 requests 65 degrees of roll.  Saturation is applied to the RC commands to keep them within the limits before they are passed into the FBWA mode, even though saturation is also applied in the aircraft simulation. This limiting action avoids a crash that can occur if very large commands are passed.

\subsubsection{Internal Autopilot Functions}
Once a command is passed to the FBWA mode, the autopilot uses its own internal PID control loops to generate the appropriate RC servo commands. The gains for these controls are set to a default which provides stable but very poor control. In order to improve performance, these gains must be tuned. This tuning was conducted using the autotune function of the Pixhawk with a custom Python script which commanded a series of very hard roll and pitch commands required by the autotune mode to select the gains. Two autotune iterations were run to generate the gains used in simulation. While autotune gains are not as good as those that are refined by manual tuning, the limitations of the available input devices into the SITL simulation made manual tuning impractical.

\subsubsection{Reliance on Trim States}
In order to set up the control laws, the trim state of the aircraft was needed, specifically trim pitch and trim throttle conditions (as trim roll for an undamaged symmetric aircraft should be zero). These states were determined by passing various simple commands to the aircraft and attempting to achieve steady level flight at the desired airspeed. Given the input limitations, trimming proved challenging and the final values selected provided adequate performance, but likely differ significantly from the true values for a real aircraft.

\section{MATLAB Simulation Results}
The Park algorithm was validated using several different ellipses and starting points using a unicycle model of the aircraft dynamics. The Simulink model is presented as \Figure{park_simulink}. $L_1$ was set to 10 meters, and the tracking speed parameter was set to be 20 m/s.  In each case, the aircraft approached the path and then tracked it with an average horizontal distance error of about 1 meter. A representative flight path is depicted in \Figure{park_ellipse}, with the associated off-track error in \Figure{park_ellipse_error}.
\centerfig{figures/ParkSimulink.pdf}{park_simulink}{MATLAB Park Algorithm Simulink Block Diagram} 
\sidebyside{figures/park_tilt_ellipse_path-crop.pdf}{park_ellipse}{Quality of Aircraft Tracking Over a Tilted Elliptical Flight Path}
{figures/park_tilt_ellipse_error-crop.pdf}{park_ellipse_error}{Off-Track Error While Tracking a Tilted Elliptical Flight Path}

\section{Architecture Block Diagrams}
Our guidance code is implemented at a high level, running aboard the supervisory computer on-board the aircraft, as shown in \Figure{sys-arch}. Control over our guidance code is accomplished via the WiFi link to the onboard computer, while the guidance code communicates via MAVLink to the Pixhawk autopilot. From the autopilot's perspective, the guidance code is simply another MAVLink-compliant message client. 
\centerfig{figures/System-Architecture.pdf}{sys-arch}{Overall System Architecture}
\subsection{Pixhawk Autopilot}
In our system, the Pixhawk autopilot is running an unmodified ArduPilot firmware. This is beneficial since the guidance algorithm implementation can theoretically be used with any autopilot compatible with MAVLink.
\subsection{Ground Control Station}
The ground control station (GCS) is assumed to be running a MAVLink-compatible planner software, such as \textit{APMPlanner2}. Should the operations team decide to utilize a GCS heartbeat check, the heartbeat will be provided by the GCS MAVLink client so as to detect problems in the 915MHz serial link between the vehicle and the ground. 
\subsection{Supervisory Computer}
The supervisory computer is running a distribution of Linux with Python 2.7 and USB CDC ACM serial support, as well as a WiFi link. The bidirectional MAVLink connection to the Pixhawk provides state data, as well as a means of transmitting control messages. Onboard the supervisory computer, the guidance code runs as a DroneAPI module, executed as a MAVProxy plugin.

\subsection{Processing Breakdown}
The navigation module consists of a number of Python modules running within DroneAPI that implements all required code. 
\subsubsection{Point Reading}
The navigation module assumes that the path to follow meets the format presented by Dr. Frew in class, in a comma-separated-value (CSV) formatted ASCII file. The points along the path are presented in a local east-north-up coordinate frame and are assumed to wrap together to form a closed path. A conversion routine translates these points into a north-east-down frame.
\subsubsection{GPS Point Translation}
The GPS point translation module transforms the current pose of the aircraft in the world frame (given by the autopilot's extended Kalman filter) into the local frame set by Dr. Frew's datum.
  
\subsubsection{Guidance Command Generation}
The guidance command generation module uses the Park $L_1$ algorithm to generate a desired yaw for the aircraft, given the current pose in local coordinates and the path to be followed as a set of discrete points in local coordinates.

\subsubsection{Flight Directive Generation}
Given a desired yaw angle and a desired altitude, the flight directive generator implements several controllers to reach the desired references. These controllers prepare a command vector to be given to the autopilot under fly-by-wire-A. 
 
\subsection{Algorithm Initiation Plan}
Our guidance logic assumes that the aircraft is already in flight in the MANUAL flight mode. When the navigation module is started, the flight mode is changed to fly-by-wire-A (FBWA), and the navigation module begins sending appropriate commands. When navigation testing is concluded, the flight mode is changed to MANUAL and the navigation module is shut down.

\subsubsection{Preflight Actions}
\begin{enumerate}
	\item Start the ODroid supervisory computer
	\item Start a MAVProxy session
	\item Disable heartbeat generation in the MAVProxy session: \\
	\textit{set heartbeat 0}
	\item Verify communication with the Pixhawk by confirming parameters and sensor values
	\item Load the DroneAPI module \\
	\textit{module load api}
	\item Standby for guidance activation from operations team
\end{enumerate}
\subsubsection{Takeoff to Activation Actions}
\begin{enumerate}
	\item Monitor the MAVProxy console for any off-nominal conditions
\end{enumerate}
\subsubsection{Guidance Activate Actions}
\begin{enumerate}
	\item Start the navigation module within DroneAPI: \\
	\textit{api start ./suas-guidance/controller.py}
	\item Report navigation engaged to operations team
\end{enumerate}
\subsubsection{Guidance Disengage Actions}
\begin{enumerate}
	\item Reset the flight mode to MANUAL: \\
	\textit{mode MANUAL}
	\item Stop the navigation module: \\
	\textit{api stop \textless threadid\textgreater}
	\item Report navigation disengaged to operations team
\end{enumerate}

\subsection{Lost Communications Plan}
The lost communications plan allows our controller to command an aerodynamic termination in the event that signals from the RC radio are not received. The \textbf{DroneAPI} source was modified to present the \textit{rssi} field to client code, representing the received signal strength indication as measured by the Pixhawk autopilot. When the \textit{rssi} drops below a certain threshold, fly-by-wire 'A' mode is engaged, with a throttle override set to the minimum value. The controller then stops sending any additional commands, but stays on-line to monitor whether communications is re-established. If the RC link is re-established, the code can disengage the aerodynamic termination override and set the autopilot back to 'MANUAL' mode, where the RC radio commands are passed through to the control surface servos.
\subsection{Software Sections}

The guidance and control algorithms are implemented entirely on the supervisory computer communicating to the Pixhawk and are written in Python 2.7. The supervisory computer is running a MAVProxy instance running the DroneAPI module to parse the telemetry information returned via the MAVLink protocol and to send RC override commands. The algorithm uses the fly-by-wire 'A' (FBWA) mode to send RC override commands for the bank angle, pitch angle, and throttle which are generally sent directly from the RC transmitter. \Figure{soft-arch} displays a flow diagram of the logic implemented in the algorithm. 

\centerfig{figures/Controller_Diagram.png}{soft-arch}{Guidance Code Architecture}

\subsubsection{Initialization}

The control script first sets the pulse width modulation (PWM) limits of the RC override for the bank angle, pitch angle, and throttle commands in FBWA mode. The PWM limits and the corresponding physical limits are shown in \tableref{pwm_limits}.

\newtable{pwm_limits}{ l| c | c | c | c| c | c}{RC PWM Limits}{
	
     RC Channel & $RC_{min}$ & $RC_{zero}$ & $RC_{max}$ & Min. Value & Zero Value & Max. Value \\
    \hline \hline
    Bank angle (RC1) & 1000 & 1500 & 2000 & $-65^{\circ}$ & $0^{\circ}$ & $65^{\circ}$\\
    \hline
    
    Pitch angle (RC2) & 1000 & 1500 & 2000 & $-30^{\circ}$ & $0^{\circ}$ & $30^{\circ}$\\
    \hline
    
    Throttle (RC3) & 1000 & 1500 & 2000 & $0\%$ & $50\%$ & $100\%$ \\
    \hline
}

Initially, the gain values on the pitch and roll servos resulted in very poor performance of the altitude and roll angle controllers. The Pixhawk's AUTOTUNE mode was used to achieve appropriate gains for the aileron and the elevator, resulting in the values seen in \tableref{servogains}. The process for using AUTOTUNE is discussed in the SITL Simulation Results section.

\newtable{servogains}{ l| c | c }{Servo Gain Values}{	
      & Roll to Servo & Pitch to Servo \\
    \hline \hline
    $K_P$ & 1.961 & 3.321\\
    \hline
    
    $K_I$ & 0.133 & 0.207 \\
    \hline
    
    $K_D$ & 0.137 & 0.232 \\
    \hline
}

Finally, the gains for the altitude controller, speed controller, and turn rate controller are set. The altitude controller uses proportional, integral and derivative (PID) control with climb rate feed forward. The speed controller and turn rate controller use proportional control with a feed-forward term.
The aircraft trim condition was then set to the trim pitch angle $\theta_{trim} = 0.989^\circ$ and the trim throttle $T_{trim} = 1230\text{ } (23\%)$. The simulation is initiated at a latitude of $40.1447601^\circ$, a longitude of $-105.2435532^\circ$, and an altitude of $ 1680.38$ meters. Once the RC limits and controller gains are set, the guidance script then changes the autopilot mode to fly-by-wire-A (FBWA) to begin tracking the desired path.

\subsubsection{Coordinate Transformation}

A critical aspect of the guidance logic is knowledge of the aircraft's position and velocity in the same frame that the desired path is described in. This requires converting the latitude $\lambda$, longitude $\Phi$, and altitude $h$ coordinates provided by the aircraft's GPS receiver to north-east-down coordinate frame centered at the origin of the desired path provided in the CSV file. The algorithm described in the reference "Conversion of Geodetic Coordinates to the Local Tangent Plane" is used to convert geodetic coordinates to earth-centered-earth-fixed (ECEF) coordinates and then translating to a local north-east down frame. First, the ellipsoid flatness of the Earth is $f=1/298.257223563$ and the eccentricity is calculated as
\begin{equation}
e=\sqrt{f(2f)}=8.18\times10^{-2}
\end{equation}

The distance from the surface to the z-axis along the ellipsoid normal is

\begin{equation}
N(\lambda)=\frac{a}{\sqrt{1-e^2\sin^2{\lambda}}}
\end{equation}

ECEF coordinates can then be caclulated as
\begin{equation}
x=(h+N)\cos{\lambda}\cos{\Phi} \\
y=(h+N)\cos{\lambda}\sin{\Phi} \\
z=(h+(1-e^2)N)\sin(\lambda)
\end{equation}

The ECEF coordinates are then translated to the local coordinate frame by subtracting the origin location $\bold{x_0}$ as

\begin{equation}
\bold{x\prime} = 
	\begin{pmatrix}
    	x \\
        y \\ 
        z \\
    \end{pmatrix}
    -
    \begin{pmatrix}
    	x_0 \\
        y_0 \\ 
        z_0 \\
    \end{pmatrix}
\end{equation}
   
   
   Then $\bold{x\prime}$ is transformed into esat-north-up coordinates by the rotation matrix $R_e^t$
   
   \begin{equation}
   	R_e^t = 
    	\begin{bmatrix}
        -\sin{\Phi_0} & \cos{\Phi_0} & 0 \\
        -\cos{\Phi_0}\sin{\lambda_0} & -\sin{\Phi_0}\sin{\lambda_0} & \cos{\lambda_0}\\
        \cos{\Phi_0}\cos{\lambda_0} & \cos{\Phi_0}\sin{\lambda_0} & \sin{\lambda_0}\\
        \end{bmatrix}
    \end{equation}
    
    Coordinates in the east-north-up frame are then defined by
    
    \begin{equation}
    \begin{pmatrix}
    	e \\ 
        n \\
        u \\
    \end{pmatrix}
    =[R_e^t]\bold{x\prime}
    \end{equation}
    
    A simple rotation is then required to transform into north-east-down coordinates.

\subsubsection{Guidance Logic}

The heart of the control and guidance logic implemented in DroneAPI is the nonlinear guidance algorithm for trajectory tracking described by Park et. al. The flow diagram in \Figure{park} displays the logic implemented in software. 

\centerfig{figures/Park_Algorithm_Diagram.png}{park}{Nonlinear Guidance Logic Diagram}

First, the closest point on the desired path to the aircraft is determined. If the closest point is greater than $L_1$, then the closest point is taken as the reference point. Otherwise, the algorithm steps along the desired path to find the point that is closest to $L_1$ distance away, which is taken as the reference point. An $L_1$ value of 168 meters was used. The $L_1$ vector is then formed in two dimensions as

\begin{equation}
	L_1 = 
    \begin{pmatrix}
    	x_{ref}\\
        y_{ref}
    \end{pmatrix}
    -
    \begin{pmatrix}
    	x_{AC}\\
        y_{AC}
    \end{pmatrix}
\end{equation}

Next, the angle $\eta$ between the aircrafts heading and the $L_1$ vector is determined by

\begin{equation}
	\eta = \arctan{\frac{L_{1,y}}{L_{1,x}}} - \chi
\end{equation}

The desired lateral acceleration is then determined by

\begin{equation}
	a_{s,cmd} = 2\frac{v^2}{L_1}\sin{\eta}
    \end{equation}
    
    By relating this lateral acceleration to the centripetal acceleration, the instantaneous radius of the required path is
    
    \begin{equation}
    R = \frac{L_1}{2\sin{\eta}}
    \end{equation}
    
    The desired turn rate is then found by
    
    \begin{equation}
    \dot{\chi} = \frac{V}{R}
    \end{equation}
\subsubsection{Roll Angle Controller}

The guidance logic described by Park outputs a desired turn rate $\dot{\chi}_{des}$ required to place the aircraft on the desired path. The turn rate controller uses proportional control on the error between desired and current turn rate with a feed forward term based on the aircraft velocity and desired turn rate. The proportional gain is $K_{P,\phi} = 13.0$  deg/(rad/s), and the feed-forward term is determined by


\begin{equation}
	\phi_{FF} = \arctan{(\dot{\chi}_{des}*v_{airspeed}/g)}
\end{equation}

where $g=9.81 m/s^2$ is the acceleration due to gravity. The commanded bank angle is then determined by

\begin{equation}
\label{eqn:rollcmd}
	\phi_{cmd} = \phi_{FF} + K_{P,\phi}*(\dot{\chi} - \dot{\chi}_{des})
\end{equation}

The turn rate $\dot{\chi}$ is equal to the yaw rate $\dot{\psi}$ of the aircraft, and can be obtained by rotating the aircrafts angular velocity from the body frame to the inertial frame. The rotation matrix from the inertial frame to the body frame is a function of the pitch angle $\theta$, the roll angle $\phi$ and the yaw angle $\psi$ and is given by


\begin{equation}
\label{eqn:rot_mat}
[R_i^b] = 
	\begin{bmatrix}
		\cos{\theta}*\cos{\psi} & \cos{\theta}*\sin{\psi} & 					-\sin{\theta}\\
		\sin{\phi}* \sin{\theta}*\cos{\psi} - \cos{\phi}*\sin{\psi} & 			\sin{\phi}* \sin{\theta}*\sin{\psi} + \cos{\phi}*\cos{\psi} & 			\sin{\phi}*\cos{\theta} \\
		\cos{\phi}* \sin{\theta}*\cos{\psi} + \sin{\phi}*\sin{\psi} & 			\cos{\phi}* \sin{\theta}*\sin{\psi} - \sin{\phi}*\cos{\psi} & 			\cos{\phi}*\cos{\theta} 
	\end{bmatrix}
\end{equation}


The rotation matrix from body frame coordinates to inertial frame coordinates is obtained by the transposing $R_b^i$, thus the Euler angle rates are given by

\begin{equation}
	\begin{bmatrix}
    \dot{\phi}\\
    \dot{\theta}\\
    \dot{\psi}
    \end{bmatrix}
    =[R_i^b]^T*\omega
\end{equation}

The bank angle command calculated in Equation \eqref{eqn:rollcmd} is then translated into RC stick values as
\begin{equation}
RC1_{cmd} = 50*\phi_{cmd}*(RC1_{max} - RC1_{min})/\phi_{lim} + RC1_{zero}
\end{equation}

\subsubsection{Altitude Controller}

The guidance logic obtains the desired altitude $h_{des}$ of the aircraft directly from the z-component in the CSV file describing the desired path. This value is then passed to the altitude controller implemented in the DroneAPI script. The altitude controller uses PID control with a feed-forward term on the pitch. The gains are shown in \tableref{alt_gains}.
\newtable{alt_gains}{ l| c | r}{Altitude Gains}{

    $K_{P,\theta}$ & $K_{I,\theta}$ & $K_{D,\theta}$ \\
    \hline \hline
    0.3 & 0.03 & 0.6 \\
    \hline
}

The climb-rate feed-forward term is calculated by predicting the aircraft's future state based upon the current position and velocity as

\begin{equation}
X_{FF} = X_{AC} + v_{airspeed}*\cos{\chi}*\Delta t
\newline
Y_{FF} = Y_{AC} + v_{airspeed}*\sin{\chi}*\Delta t
\newline
Z_{FF} = Z_{AC} + v_z*\Delta t
\end{equation}

The pitch angle commanded $\theta_{cmd}$ is given by

\begin{equation} \theta_{cmd} = K_{P,\theta}(h_{des} - h_{AC}) - K_{I,\theta}*h_{I,err} - K_{D,\theta}(v_z - \dot{h}_{des}) - \theta_{trim}
\end{equation}


The commanded pitch angle must then be converted into a PWM value to use the RC override command. For a positive commanded pitch angle, the RC override is determined by

\begin{equation}
RC2_{cmd} = -100*\theta_{cmd}(RC2_{zero} - RC2_{min})/(\theta_{max}) + RC2_{zero}
\end{equation}

For a negative commanded pitch angle, the RC override is determined by

\begin{equation}
RC2_{cmd} = -100*\theta_{cmd}(RC2_{max} - RC2_{zero})/(-\theta_{max}) + RC2_{zero}
\end{equation}

\subsubsection{Speed Controller}

The speed of the aircraft is desired to remain constant throughout the manuever, with a desired speed of $v_{des} = 22 m/s$. In order to achieve this, a proportional controller is implemented with a feed-forward term on the pitch rate. The proportional gain for the throttle is $K_{P,T} = 4.0$ and the feed-forward gain is $K_{FF,T} = 350.0$. The throttle command $T_{cmd}$ is given by

\begin{equation}
T_{cmd} = K_{P,T}(v_{des} - v_{airspeed}) + K_{FF,T}\sin{\theta - \theta_{trim}}
\end{equation}

The commanded throttle is then converted to a RC override command as

\begin{equation}
RC3_{cmd} = 10*T_{cmd} + T_{trim}
\end{equation}




\subsubsection{Saturation Limits}


The RC override commands provided by the individual controllers discussed previously are then saturated to the PWM limits to avoid sending out of range RC values to the Pixhawk.

\begin{equation}
	RCi_{cmd} = 
	\begin{cases}
		RCi_{max} & \text{if } RCi_{cmd} > RCi_{max}\\
        RCi_{min} & \text{if } RCi_{cmd} < RCi_{min}\\
        RCi_{cmd} & \text{otherwise}
     \end{cases}
     \text{      for i=(1,2,3)}
\end{equation}

\subsubsection{DroneAPI Modifications}
In order to provide state information necessary for our implementation of the Park algorithm, the DroneAPI code had to be modified. The default ArduPilot firmware provides a rich telemetry set, but several key parameters are not exposed to modules running within the DroneAPI framework. Minor changes were required to expose the following parameters:
\begin{enumerate}
	\item \textit{rssi}: The radio signal strength indication parameter was necessary to implement the lost communications plan
	\item \textit{pitchspeed}, \textit{yawspeed}, and \textit{rollspeed}: The angular rate information was necessary to assist the command translation module in generating appropriate RC commands.

\end{enumerate}

\section{SITL Simulation Results}
Several SITL simulation results are included, starting the aircraft from a different initial position. Each results concludes with the aircraft reaching the path. The tracking results are from the entire telemetry file, including an initial portion where the aircraft was travelling to the starting position, at which point the guidance controller was engaged. The error plots are limited to the simulation results where the aircraft has acquired the path.
\subsection{Start: Inside The Chip}
\centerfig{figures/inside_chip_path-crop.pdf}{inside_chip}{Tracking performance starting inside the chip}
\centerfig{figures/inside_chip_path_error-crop.pdf}{inside_chip_error}{Off-Track error, starting inside the chip}
\subsection{Start: Outside The Chip}
\centerfig{figures/outside_chip_path-crop.pdf}{outside_chip}{Tracking performance starting outside the chip}
\centerfig{figures/outside_chip_path_error-crop.pdf}{outside_chip_error}{Off-Track error, starting outside the chip}
\subsection{Start: Above and Outside The Chip}
\centerfig{figures/above_outside_chip_path-crop.pdf}{above_outside_chip}{Tracking performance starting above and outside the chip}
\centerfig{figures/above_outside_chip_path_error-crop.pdf}{above_outside_chip_error}{Off-Track error, starting above and outside the chip}
\subsection{Discussion of SITL Results}
Reviewing the tracking results, it appears that the Park algorithm is capable of tracking the potato chip path within several meters. In the off-track error plots, one can see a periodic increase in the error corresponding to the highest elevations of the potato chip desired path. This error pattern is visible across each of the test runs.
\subsection{Autotune}
The default gains for the roll and pitch to servo actuations present in the simulation resulted in poor tracking performance of the desired trajectory. To determine the gains required for good tracking performance, the autopilot's 'AUTOTUNE' mode was used. A Python script was written to command a series of roll-left and roll-right commands followed by a series of pitch up and pitch down commands. 'AUTOTUNE' mode, as the name suggests, modifies the servo gains during flight to achieve a well-damped response to these roll and pitch motions. The resulting gains were then extracted from the autopilot parameters, and the process was repeated for these new gains; the intermediate and final values are shown in \tableref{autotune}.

\newtable{autotune}{ l| c | c}{Autotune-extracted Servo Gains}{
	&Roll to Servo & Pitch to Servo \\
    
    
    \hline 
    \hline \hline
    Run 1 & & \\
    \hline \hline
    $K_P$ & 1.894 & 2.959 \\
    \hline
    $K_I$ & 0.175 & 0.175 \\
    \hline
    $K_D$ & 0.133 & 0.207 \\
    \hline \hline
    Run 2 & & \\
    \hline \hline
    $K_P$ & 1.961 & 3.321 \\
    \hline
    $K_I$ & 0.133 & 0.207 \\
    \hline
    $K_D$ & 0.137 & 0.232

}

Note that these gains resulted in good performance for the simulated vehicle. The gains should be tuned in a similar fashion during actual flight tests.

\subsection{Lost Communications}
In simulation, the \textit{rssi} field is set to 0; we were able to verify the correct performance of a proof-of-concept implementation of the lost comm strategy. In simulation, the aircraft glides to the ground. This section would require additional verification with the RC remote and physical system before integrating onto an airframe.
\section{Recommendations}
Several practical implementation challenges were encountered during development of the path following controller in a open-source SITL environment.
\subsection{Aerodynamics Model}
The aircraft loaded into JSBSim engine underlying the APM SITL simulation is a Rascal 110 model aircraft. However, neither the aerodynamic properties nor plant models were available to perform initial controller testing in a more controlled software environment. This forced our team to develop and tune outer-loop controllers with a nonlinear plant in a complete flight dynamics simulator. To improve outer-loop control when using FBWA mode, AUTOTUNE proved very useful in providing rough gains for the inner-loop controls that manage bank angle and pitch. Although not as ideal as flying the actual aircraft and tuning while in the air, the AUTOTUNE mode satisfactorily improved the performance of AUTO mode. The STABILIZE and FBWB modes were used to obtain a trim pitch and throttle setting for the aircraft which also helped improve gain tuning.
\subsection{Wind Model}
 Our team was also not able to determine how to disable wind in the simulator. This wind was based on a turbulence algorithm that varied magnitude and heading as function of the aircraft airspeed and altitude. Since our controller logic did not contain any explicit disturbance rejection, this wind added challenges to creating an optimal baseline for gain tuning. In a sense, our controller contains a rough estimate of disturbance rejection by tuning the gains about the random wind distribution. However, a more optimal controller may be possible if tuned without wind initially.

\subsection{Use of FBWB Mode}
The FBWB mode was pursued initially for its ability to automatically hold altitude commands. Upon further investigation of the logic behind the altitude controller, it was determined that the FBWB mode is a poor choice for path following applications. The altitude controller is slow at climbing and descending, making it a poor choice for 3D path tracking.  It is strongly recommended to implement the path following controller in FBWA mode or MANUAL mode only.

\subsection{Evaluation of DroneAPI}
DroneAPI proved to be an able platform to implement a path following controller. Modifying the code to access additional telemetry was a simple task and implementing the script in Python was simple and easy to debug. Although an implementation in firmware (C++) would certainly yield higher controller performance, the ability to avoid breaking C++ inter-dependencies in the APM source code was useful. Additionally, the APM source was not required to attempt different controller techniques and therefore only the APM binaries are needed for controller testing. However there still exists a few quirks in the DroneAPI script as the project is still in its infancy; these quirks relate to multithreading and cross-thread data access. The SITL APM module does not return ACKs for commands sent through MAVLink, and thus also for DroneAPI. Therefore, if you set a parameter in your script (rather than hardcoding it to a file and loading via the virtual EEPROM), then DroneAPI will stall for approximately two seconds until a timeout occurs. The parameter is still set, but this delay caused a significant challenge to quick fix testing in the controller code. The DroneAPI project lead was made aware of this issue, and certain lines were commented out to prevent the timeout loop from executing as an interim solution.

\subsection{Use of APM Planner 2.0}
APM Planner 2.0 is an excellent graphical interface tool to view SITL data during testing. However, the application is still very buggy and has a few quirks. These bugs have been made aware to the lead developers via online forums. Through testing, QGroundControl, a branch of APM Planner 1.0 is even less stable and less compatible with APM SITL, and should be avoided.

\subsection{Choice of Linux Distribution}
The operating system that presented the least number of issues loading all the required APM SITL packages was Ubuntu. This is because Ubuntu is a relatively package-laden operating system that is easily supported by developers. Python 2.7 is pre-installed with the required packages, and minimizes the amount of time the user must spend installing missing dependencies. The operating system is also the target for APM Planner, APM SITL, and DroneAPI tutorials, making them trivial to follow. Given that each of the components utilizes common components, using alternative Linux distributions does not pose a challenge for a sufficiently knowledgeable user.

\subsection{Nonlinear Paths}
Our controller logic required extensive trial and error testing to determine the appropriate controller type for the path following problem. It was determined that PID controllers were required for both bank angle and pitch control because our path was not linear. This meant that proportional-derivative control would not be suitable to obtain a near-zero steady state error. However, as discussed earlier, this meant that integrator wind-up would need to be addressed, particularly at the apexes of the desired path.

\section{References}
\textit{Conversion of Geodetic coordinates to the Local Tangent Plane}, Portland State Aerospace Society, Version 2.01, URL: \url{http://psas.pdx.edu/CoordinateSystem/Latitude\_to\_LocalTangent.pdf} [cited 23 October 2014].

Park., S., Deyst, J., and How, J. P., \textit{A New Nonlinear Guidance Logic for Trajectory Tracking}, AIAA Guidance, Navigation and Control Conference, 2004.
\newpage
\section{Appendix A: Code}
\python{Controller.py: Main controller implementation}{../controller/controller.py}
\newpage
\python{Guide.py: Implementation of Nonlinear Guidance Code}{../controller/L1Guide.py}

\end{document}